#include "rclcpp/rclcpp.hpp"
#include "rms_pkg/msg/heartbeat.hpp"
#include "../include/global_logger.h"
#include <chrono>
#include <algorithm>  // for std::find
#include <memory>

using namespace std::chrono_literals;

class HeartbeatPublisher : public rclcpp::Node {
public:
    HeartbeatPublisher() : Node("heartbeat_publisher") {
        // 获取全局日志记录器
        logger_ = GLOBAL_LOGGER;
        LOG_INFO(" HeartbeatPublisher 初始化开始");

        // 声明参数
        this->declare_parameter<std::string>("robot_id", "robot001");
        this->declare_parameter<std::string>("robot_type", "inspection");  // 默认值修正
        this->declare_parameter<int>("interval", 1);
        this->declare_parameter<float_t>("x",1.0);
        this->declare_parameter<float_t>("y",1.0);
        this->declare_parameter<std::string>("seer_ip","192.168.88.103");
        this->declare_parameter<std::string>("camera_url","rtsp://admin:Lssec_12@192.168.88.103:554/h265/ch1/main/av_stream");

        // 获取参数
        this->get_parameter("robot_id", robot_id_);
        this->get_parameter("robot_type", robot_type_);
        int interval = 1;
        this->get_parameter("interval", interval);
        if (interval <= 0) {
            LOG_WARN(" 无效的时间间隔，使用默认值 1s");
            interval = 1;
        }
        this->get_parameter("x", x_);
        this->get_parameter("y", y_);
        this->get_parameter("seer_ip", seer_ip_);
        this->get_parameter("camera_url", camera_url_);

        // 参数验证（可选）
        std::vector<std::string> valid_types = {"inspection", "harvest", "weed", "grafting", "transport", "guide"};
        if (std::find(valid_types.begin(), valid_types.end(), robot_type_) == valid_types.end()) {
            LOG_WARN(" 无效的 robot_type: {}，使用默认值 'inspection'", robot_type_);
            robot_type_ = "inspection";
        }

        // 创建发布者
        auto qos = rclcpp::QoS(10).reliability(RMW_QOS_POLICY_RELIABILITY_RELIABLE);
        publisher_ = this->create_publisher<rms_pkg::msg::Heartbeat>("heartbeat", qos);

        // 定时器
        timer_ = this->create_wall_timer(
            std::chrono::seconds(interval),
            [this]() {
                this->timer_callback();
            });

        LOG_INFO(" HeartbeatPublisher 初始化完成! 创建心跳发布者，间隔: {}s", interval);
    }

private:
    void timer_callback() {
        auto msg = rms_pkg::msg::Heartbeat();
        msg.robot_id = robot_id_;
        msg.robot_type = robot_type_;
        msg.x = x_;
        msg.y = y_;
        msg.status = "idle";  // 示例状态
        msg.battery = 100;  // 电量百分比（int8类型）
        msg.seer_ip = seer_ip_;
        msg.camera_url = camera_url_;
        
        // 使用 steady_clock 替代 system_clock，避免系统时间调整影响
        msg.timestamp = static_cast<uint64_t>(
        std::chrono::duration_cast<std::chrono::microseconds>(
            std::chrono::steady_clock::now().time_since_epoch()
        ).count()
    );
        publisher_->publish(msg);
        // 添加更多信息到日志，便于调试
        uint64_t timestamp_ms = msg.timestamp / 1000;
        LOG_DEBUG(" 发送心跳包: {} ({}) 时间戳: {} ms", 
                    robot_id_, robot_type_, timestamp_ms);
    }

    rclcpp::Publisher<rms_pkg::msg::Heartbeat>::SharedPtr publisher_;
    rclcpp::TimerBase::SharedPtr timer_;
    std::string robot_id_;
    std::string robot_type_;
    std::string seer_ip_;
    std::string camera_url_;
    float_t x_, y_;

    // 日志记录器
    std::shared_ptr<spdlog::logger> logger_;
};

int main(int argc, char *argv[]) {
    rclcpp::init(argc, argv);
    // 初始化全局日志记录器（如果尚未初始化）
    GlobalLogger::getInstance().initialize("log", "RMSClient_heartbeat", 5);
    rclcpp::spin(std::make_shared<HeartbeatPublisher>());
    rclcpp::shutdown();
    return 0;
}
